#include "stdafx.h"
#include "CalRoadZCurve.h"
#include "TerrianMesh.h"

void CalRoadZCurve::Cal(double dMaxDetalHeight, double dMinCalDis, double dDetalHeight)
{
	for (vector<PolyPnt>& vPnt : m_vRoadMiddleLine)
	{
		vector<bool> vIsUse(vPnt.size(), false);
		vector<double> vLength(vPnt.size(), 0);
		for (int i=1;i< vPnt.size();++i)
		{
			vLength[i] = vLength[i-1] + VeLength2(vPnt[i].GetPnt() - vPnt[i-1].GetPnt());
		}
		
		bool isFind = false;
		for (int i=0;i< vPnt.size(); ++i)
		{
			Point_3 pntOnTerrain;
			if (!CalHeight(pntOnTerrain, vPnt[i].GetPnt(), dDetalHeight))
			{
				isFind = true;
				for (int j=0; j<=i; ++j)
				{
					Point_3 pnt = vPnt[j].GetPnt();
					vPnt[j].SetPnt(Point_3(pnt[0],pnt[1], pntOnTerrain[2]));
				}
				break;
			}
		}
		if(!isFind) continue;
		isFind = false;
		for (int i = vPnt.size() - 1; i >= 0; --i)
		{
			Point_3 pntOnTerrain;
			if (!CalHeight(pntOnTerrain, vPnt[i].GetPnt(), dDetalHeight))
			{
				isFind = true;
				for (int j = i; j <= vPnt.size() - 1; ++j)
				{
					Point_3 pnt = vPnt[j].GetPnt();
					vPnt[j].SetPnt(Point_3(pnt[0], pnt[1], pntOnTerrain[2]));
				}
				break;
			}
		}
		if (!isFind) continue;

		stack<pair<int, int>> vStack;
		vStack.push(make_pair(0, vPnt.size() - 1));
		while (!vStack.empty())
		{
			pair<int, int> paData = vStack.top(); vStack.pop();
			int iFrom = paData.first;
			int iTo = paData.second;
			Point_3 pntFrom = vPnt[iFrom].GetPnt();
			Point_3 pntTo = vPnt[iTo].GetPnt();
			double dLength = vLength[iTo] - vLength[iFrom];
			int iMiddle = (iFrom + iTo) / 2;
			double dLengthMiddle = vLength[iMiddle] - vLength[iFrom];
			Point_3 pntMiddle = vPnt[iMiddle].GetPnt();
			double zEval, zTerrain;
			zEval = ((pntTo[2] - pntFrom[2]) * dLengthMiddle / dLength) + pntFrom[2];
			if (dLength > dMinCalDis)
			{
				if (!m_pTerrain->GetHeight(zTerrain, pntMiddle[0], pntMiddle[1]))
				{
					if (fabs(zEval - zTerrain) > dMaxDetalHeight)
					{
						zEval = zEval > zTerrain ? zTerrain + dMaxDetalHeight : zTerrain - dMaxDetalHeight;
					}
				}
				
			}
			
			vPnt[iMiddle].SetPnt(Point_3(pntMiddle[0], pntMiddle[1], zEval));
			vIsUse[iMiddle] = true;
			if (iMiddle - iFrom > 1)
			{
				vStack.push(make_pair(iFrom, iMiddle));
			}
			if (iTo - iMiddle > 1)
			{
				vStack.push(make_pair(iMiddle, iTo));
			}
		} 
	}
}

int CalRoadZCurve::CalHeight(Point_3& pntCal, const Point_3& pnt, double dDetalHeight)
{
	double x, y, z;
	x = pnt[0];		y = pnt[1];
	int iErr = m_pTerrain->GetHeight(z, x, y);
	if (!iErr)
	{
		z += dDetalHeight;
		pntCal = Point_3(x, y, z);
	}
	return iErr;
}
